#include "stm32f4xx.h"                  // Device header
#include "stm32f4xx_conf.h"
#include "GO_M8010_6.h"
#include "RS485.h"
#include "CRC.h"

#define PI 3.141592653589793238462643383279f

GO_M8010_6_Motor GO_M8010_6_MotorStatus;
uint8_t RS485_RxData;
uint8_t RS485_RxHEXPacket[16]={0xFD,0xEE,0x00};
int16_t Checksum;
float StartPosition;


uint8_t D[15]={0xFE,0xEE};

void GO_M8010_6_Send(uint8_t ID,float Tau,float w,float Theta,float Kp,float Kw)
{
	uint8_t R[2];
	CRC_CRCTypeDef GO;
	CRC_StructureInit(&GO,CRC_16_CCITT);
	
	D[2]=ID<<4;
	D[3]=(uint16_t)((int16_t)(Tau*256)) & 0x00FF;
	D[4]=((uint16_t)((int16_t)(Tau*256)) & 0xFF00)>>8;
	D[5]=(uint16_t)((int16_t)(w*256/(2*PI))) & 0x00FF;
	D[6]=((uint16_t)((int16_t)(w*256/(2*PI))) & 0xFF00)>>8;
	D[7]=(uint32_t)((int32_t)(Theta*32768/(2*PI))) & 0x000000FF;
	D[8]=((uint32_t)((int32_t)(Theta*32768/(2*PI))) & 0x0000FF00)>>8;
	D[9]=((uint32_t)((int32_t)(Theta*32768/(2*PI))) & 0x00FF0000)>>16;
	D[10]=((uint32_t)((int32_t)(Theta*32768/(2*PI))) & 0xFF000000)>>24;
	D[11]=(uint16_t)((int16_t)(Kp*1280)) & 0x00FF;
	D[12]=((uint16_t)((int16_t)(Kp*1280)) & 0xFF00)>>8;
	D[13]=(uint16_t)((int16_t)(Kw*1280)) & 0x00FF;
	D[14]=((uint16_t)((int16_t)(Kw*1280)) & 0xFF00)>>8;
	
	CRC_Calc(&GO,D,15,R);
	RS485_SendArray(D,15);
	RS485_SendArray(R,2);
}

void GO_M8010_6_ControlPosition(uint8_t ID,float Position)//弧度
{
	//GO_M8010_6_Send(ID,0,0,0,0,0);
	GO_M8010_6_Send(ID,0,0.25f,Position*6.33f-Checksum*2.0f*PI,2.0f,0.05f);
}

/*
 *函数简介:GO-M8010-6电机初始化
 *参数说明:无
 *返回类型:无
 *备注:UART2为USART1,默认Tx引脚PA9,Rx引脚PB7
 */
void GO_M8010_6_Init(void)
{
	RS485_Init();
	
	GO_M8010_6_Send(0x01,0,0,0,0,0);
	while(RS485_RxFlag==0);
	RS485_RxFlag=0;
	while(!(GO_M8010_6_MotorStatus.Theta+Checksum*2.0f*PI<0 && GO_M8010_6_MotorStatus.Theta+Checksum*2.0f*PI>=-2.0f*PI))
	{
		if(GO_M8010_6_MotorStatus.Theta>=0)Checksum--;
		else Checksum++;
	}
	StartPosition=(GO_M8010_6_MotorStatus.Theta+Checksum*2.0f*PI)/6.33f;
}

/*
 *函数简介:RS485接收中断(USART1串口中断函数)
 *参数说明:无
 *返回类型:无
 *备注:接收完成置RS485_RxFlag为1,需软件清0
 */
//**************************************************
void USART1_IRQHandler(void)
{
	static int RxHEXState=0;//定义静态变量用于接收模式的选择
	static int pRxHEXState=0;//定义静态变量用于充当计数器
	
	if(USART_GetITStatus(USART1,USART_IT_RXNE)==SET)//查询接收中断标志位
	{
		RS485_RxData=USART_ReceiveData(USART1);//将数据存入缓存区
		
		if(RxHEXState==0)//模式0-等待包头
		{
			if(RS485_RxData==0xFD)//检测包头
				RxHEXState=1;//转入模式1
		}
		else if(RxHEXState==1)//模式1-接收有效数据
		{
			if(RS485_RxData==0xEE)//检测包头
			{
				RxHEXState=2;//转入模式2
				pRxHEXState=0;//复位计数器
			}
			else
				RxHEXState=0;//转入模式2
		}
		else if(RxHEXState==2)//模式1-接收有效数据
		{
			RS485_RxHEXPacket[pRxHEXState+2]=RS485_RxData;//接收数据
			pRxHEXState++;
			
			if(pRxHEXState>=14)
			{
				RxHEXState=0;//回到模式0
				CRC_CRCTypeDef GO;
				uint8_t R[2];
				CRC_StructureInit(&GO,CRC_16_CCITT);
				CRC_Calc(&GO,RS485_RxHEXPacket,14,R);
				//if(R[0]==RS485_RxHEXPacket[14] && R[1]==RS485_RxHEXPacket[15])
				{
					GO_M8010_6_MotorStatus.ID=RS485_RxHEXPacket[2]>>4;
					GO_M8010_6_MotorStatus.STATUS=(RS485_RxHEXPacket[2]&0x0F)>>1;
					GO_M8010_6_MotorStatus.Tau=(int16_t)(((uint16_t)RS485_RxHEXPacket[4]<<8)|RS485_RxHEXPacket[3])/256.0f;
					GO_M8010_6_MotorStatus.w=(int16_t)(((uint16_t)RS485_RxHEXPacket[6]<<8)|RS485_RxHEXPacket[5])/256.0f*2.0f*PI;
					GO_M8010_6_MotorStatus.Theta=(int32_t)(((uint32_t)RS485_RxHEXPacket[10]<<24)|((uint32_t)RS485_RxHEXPacket[9]<<16)|((uint32_t)RS485_RxHEXPacket[8]<<8)|RS485_RxHEXPacket[7])/32768.0f*2.0f*PI;
					GO_M8010_6_MotorStatus.TEMP=(int8_t)RS485_RxHEXPacket[11];
					GO_M8010_6_MotorStatus.MERROR=RS485_RxHEXPacket[12]>>5;
					GO_M8010_6_MotorStatus.FORCE=((uint16_t)RS485_RxHEXPacket[13]<<4)|(RS485_RxHEXPacket[12]&0x1F);
					RS485_RxFlag=1;//置接收完成标志位
				}
			}
		}
	}
}
//**************************************************/
